#include "modules/CtrlModule/ActivePlanner/CliffFollowPlanner.h"
#include "data_struct/base/DataSet.h"
#include "lib/WrapFuncs.h"
#include "data_struct/geometry/Point.h"
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"
#include "modules/CtrlModule/MarkPosePubModel.h"

namespace behavior_controller
{
#define TURN_REEOR   0.052359876   // 0.08726646=5deg   0.052359876=3deg
CliffFollowPlanner cliff_follow_planner;

void CliffFollowPlanner::run(double &linear_vel, double &angular_vel, bool &around_finished)
{
    std::vector<PointF> mark_pose_array;
    // 60s , 0.3m -> 0.3 * 0.3 = 0.09
    if ((pow(DataSet::robot_pose.x() - start_pose.pose.position.x, 2) +
         pow(DataSet::robot_pose.y() - start_pose.pose.position.y, 2)) <= 0.09
        && Time::Now() - start_pose.header.stamp.sec > 60)
    {
        SetFinishTrueClearPose();
        around_finished = true;
    }
    // 当还没有检测到悬崖直线时
    if (!line_flag)
    {
        CliffId cliff_id = DataSet::cliff_data.GetCliffId();
        PoseF world_cliff_point;
        bool try_new_point = true; // dpj
        if (cliff_id == CliffId::RIGH_FRON)
        {
            world_cliff_point = DataSet::robot_pose.ConvertOnCur(righ_fron_cliff);
        }
        else if (cliff_id == CliffId::RIGH_MIDD)
        {
            world_cliff_point = DataSet::robot_pose.ConvertOnCur(righ_mid_cliff);
        }
        else if (cliff_id == CliffId::RIGH_BACK)
        {
            world_cliff_point = DataSet::robot_pose.ConvertOnCur(righ_back_cliff);
        }
        else if (cliff_id == CliffId::LEFT_FRON)
        {
            world_cliff_point = DataSet::robot_pose.ConvertOnCur(left_fron_cliff);
        }
        else if (cliff_id == CliffId::LEFT_MIDD)
        {
            world_cliff_point = DataSet::robot_pose.ConvertOnCur(left_mid_cliff);
        }
        else if (cliff_id == CliffId::LEFT_BACK)
        {
            world_cliff_point = DataSet::robot_pose.ConvertOnCur(left_back_cliff);
        }
        else
        {
            try_new_point = false;
        }
        std::cout << "GetCliffId = " << cliff_id << " try_new_point: " << try_new_point
                  << " point_set: " << point_set.size() << std::endl;
        if (try_new_point)
        {
            if (!point_set.empty())
            {
                bool is_push_bcak = true;
                for (const auto & i : point_set)
                {
                    if (world_cliff_point.DistanceSquare(i) < 0.004)  //  0.02m
                    {
                        is_push_bcak = false;
                        break;
                    }
                }
                if (is_push_bcak)
                {
                    point_set.emplace_back(world_cliff_point);
                    PointF robot_pose_tmp(world_cliff_point.x(), world_cliff_point.y());
                    // don not mark the first point ,because it is not correct .
                    mark_pose_array.emplace_back(robot_pose_tmp);
                    time_now = 0;
                }
            }
            else
            {
                point_set.emplace_back(world_cliff_point);
                PointF robot_pose_tmp(world_cliff_point.x(), world_cliff_point.y());
                time_now = 0;
            }
        }
        std::cout << "cliff point size= " << point_set.size() << std::endl;
        if (point_set.size() > 3) //悬崖点集大于2时，即3个悬崖点时，进行直线计算，2个但误差大,
        {                         //经测试，在探地传感器准确的情况下，3个点可以获得准确的悬崖直线
            PointCloud<double> world_point_cloud;
            for (size_t i = 1; i < point_set.size(); i++) //don not use the first point ,because it is not correct.
            {
                world_point_cloud.emplace_back(point_set[i]);
                std::cout << "cliff point x=  " << point_set[i].x() << " y= " << point_set[i].y() << std::endl;
            }
            double x_mean, y_mean;
            Funcs::LineFit(world_point_cloud, line_k, line_b, R, x_mean, y_mean);
            std::cout << "LineFit :R= " << R << " line_k= " << line_k << " line_b= " << line_b << std::endl;
            if (!std::isnan(R) && fabs(R) > 0.6)
            {
                //Funcs::GetInlier(line_k, -1, line_b, point_set[0], point_set[2], start_point, end_point);
                PoseF out_point(0.0f, 0.0f, .0f);
                PoseF robot_pose_ = DataSet::robot_pose;
                Funcs::GetInlier(line_k, -1, line_b, robot_pose_, out_point);
                angle_line_global = Funcs::NormalizeAngle(
                        std::atan2(out_point.y() - robot_pose_.y(), out_point.x() - robot_pose_.x()) +
                        M_PI_2); // + 90deg
                line_flag = true;
                TurnToAngle(linear_vel, angular_vel);
                point_set.clear();
            }
            else
            {
                std::cout << "R is bad,Back And Turn" << std::endl;
                if (try_new_point) time_now = 0;
                BackAndTurn(linear_vel, angular_vel);
            }
        }
        else
        {
            std::cout << "Back And Turn" << std::endl;
            if (try_new_point) time_now = 0;
            BackAndTurn(linear_vel, angular_vel);
        }
    }
    else //检测到悬崖直线时
    {
        std::cout << "find cliff line !" << std::endl;
        double dst_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() - angle_line_global); //机器人目标角度 dpj
        if (!turn_finished)
        {
            if (fabs(dst_angle) > TURN_REEOR) //机器人与悬崖线的夹角
            {
                TurnToAngle(linear_vel, angular_vel);
            }
            else
            {
                turn_finished = true;
            }
        }
        if (turn_finished) //机器人与悬崖线的夹角<0.03的情况,进入沿悬崖状态
        {
            if (time_now2 < 1e-6)
                time_now2 = Time::Now();
            if (Time::Now() - time_now2 <= 3) //沿悬崖行走超过5s后，退出沿悬崖，进入沿障碍物
            {
                if (DataSet::cliff_data.GetCliffId() == CliffId::RIGH_BACK) //触发右后侧的传感器后向左调整
                {
                    linear_vel = 0.2;
                    angular_vel = 0.15;
                    std::cout << "right back " << std::endl;
                } else if (DataSet::cliff_data.GetCliffId() == CliffId::RIGH_MIDD) //
                {
                    linear_vel = 0;
                    angular_vel = 0.15;
                    std::cout << "right mid " << std::endl;
                } else if (DataSet::cliff_data.GetCliffId() == CliffId::LEFT_FRON) //
                {
                    linear_vel = -0.2;
                    angular_vel = 0;
                    std::cout << "left front:  run back !" << std::endl;
                } else if (DataSet::cliff_data.GetCliffId() == CliffId::RIGH_FRON) //
                {
                    linear_vel = -0.2;
                    angular_vel = 0;
                    std::cout << "rigth front: run back !" << std::endl;
                } else
                {
                    double dis_ = Funcs::DistToLine(line_k, line_b, DataSet::robot_pose) - ROBOT_RADIUS;
                    std::cout << "dis_to_line : " << dis_ << std::endl;  //<< "  dis_2= "<< dis_2<<std::endl;
                    /*if (dis_ >= 0.25) //与悬崖线的距离>0.25
                    {
                        linear_vel = 0.2;
                        angular_vel = -0.2;
                    }
                    else if (dis_ < 0.25 && dis_ > 0.025) //与悬崖线的距离<0.25 > 0.025
                    {
                        linear_vel = 0.2;
                        angular_vel = -0.2;
                    }
                    else if (dis_ > 0.005 && dis_ < 0.025) //与悬崖线的距离 >0.005 < 0.025
                    {
                        pid_adjustment(dis_, linear_vel, angular_vel);
                    }
                    else
                    {
                        linear_vel = 0.2;
                        angular_vel = 0.2;
                    }*/
                    linear_vel = 0.2;
                    angular_vel = -0.1;
                }
            } else
            {
                time_now =
                time_now2 =
                time_now3 = 0;
                point_set.clear();
                turn_finished =
                line_flag = false;
                finished = true;
                std::cout << "Cliff Timeout set finished!" << std::endl;
            }
        }
    }
//    mark_pose_pub_mod.MarkPosePub(mark_pose_array);
}

void CliffFollowPlanner::BackAndTurn(double &linear_vel, double &angular_vel)
{
    if (time_now < 1e-6) time_now = Time::Now();
    if (Time::Now() - time_now < 0.75) //1s内后退
    {
        linear_vel = -0.2;
        angular_vel = 0;
    } else if (Time::Now() - time_now < 2.25) //第2s内左转
    {
        linear_vel = 0;
        angular_vel = 0.4;
    } else if (Time::Now() - time_now < 5) //第5s make a desition , no cliff , and finish this plan
    {
        linear_vel = 0.2;
        angular_vel = -0.1;
    } else
    {
        linear_vel =
        angular_vel = 0;
        time_now =
        time_now2 =
        time_now3 = 0;
        point_set.clear();
        finished = true;
        turn_finished =
        line_flag = false;
    }
}

void CliffFollowPlanner::TurnToAngle(double &linear_vel, double &angular_vel)
{
    double dst_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() - angle_line_global); //机器人目标角度
    std::cout << "angle_dst : " << dst_angle << " goal_angle= " << angle_line_global << " robot_angle= "
              << DataSet::robot_pose.angle() << std::endl;
    if (time_now3 < 1e-6)              //dpj
        time_now3 = Time::Now();       //dpj
    if (Time::Now() - time_now3 < 1) //
    {
        linear_vel = -0.2;
        angular_vel = 0;
        return;
    }
    if (dst_angle > 0.15 || dst_angle < -0.15) //当角度差相差0.1弧度时    1deg=0.017453292rad
    {
        linear_vel = 0;
        angular_vel = -dst_angle;
    } else if (dst_angle > TURN_REEOR && dst_angle < 0.15) //当角度差小于0.1弧度时,大于0.03弧度时
    {
        linear_vel = 0;
        angular_vel = -0.15;
    } else if (dst_angle > -0.15 && dst_angle < -TURN_REEOR) //当角度差小于0.1弧度时,大于0.03弧度时
    {
        linear_vel = 0;
        angular_vel = 0.15;
    } else
    {
        linear_vel = 0;
        angular_vel = 0;
    }
}

void CliffFollowPlanner::pid_adjustment(double &robot_dis, double &linear_vel, double &angular_vel)
{
    linear_vel = 0.2;
    double dis_p = robot_dis - CLIFF_OBSDIST;  //当dis_p>0时，应向右转
    double dis_d = dis_p - last_dis_p;        //当dis_d>0时，说明变化趋势在增大，应反作用于角速度
    angular_vel = -dis_p * 0.8 - dis_d * 0.2; //角速度输出，需要调节p d参数，使机器人行走轨迹不至于过于晃动。
    last_dis_p = dis_p;
}

void CliffFollowPlanner::SetFinishTrue()
{
    time_now =
    time_now2 =
    time_now3 = 0;
    point_set.clear();
    finished = true;
    turn_finished =
    line_flag = false;
}

void CliffFollowPlanner::SetFinishTrueClearPose()
{
    SetFinishTrue();
    start_pose = geometry_msgs::msg::PoseStamped();
    start_pose.header.stamp.sec = 0;
    start_pose.header.stamp.nanosec = 0;
}

void CliffFollowPlanner::SetStartPose()
{
    if (start_pose.header.stamp.sec > 1e-4 || start_pose.header.stamp.nanosec * 1e-9 > 1e-4) return;
    else
    {
        tf2::Quaternion quaternion_;
        double t = Time::Now();
        quaternion_.setRPY(0, 0, DataSet::robot_pose.angle());
        start_pose.pose.position.x = DataSet::robot_pose.x();
        start_pose.pose.position.y = DataSet::robot_pose.y();
        start_pose.pose.orientation.x = quaternion_.x();
        start_pose.pose.orientation.y = quaternion_.y();
        start_pose.pose.orientation.z = quaternion_.z();
        start_pose.pose.orientation.w = quaternion_.w();
        start_pose.header.stamp.sec = floor(t);
        start_pose.header.stamp.nanosec = (t - start_pose.header.stamp.sec) * 1e9;
    }
}
} // namespace behavior_controller
